#ifndef RTREE_H_INCLUDED
#define RTREE_H_INCLUDED
#define NULL 0
#include <cmath>
#include <cfloat>
#include <cassert>
#include <queue>
using namespace std;

#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) > (b) ? (a) : (b))

const int DIMS_NUM=2;
const int MIN_NODES=2;
const int SIDES_NUM=2*DIMS_NUM;
const int MAX_NODES=2*MIN_NODES;
/**
* Precomputed volumes of the unit spheres for the first few dimensions
*/
const  double UnitSphereVolumes[] = {
	0.000000, /* dimension 0 */
	2.000000, /* dimension 1 */
	3.141593, /* dimension 2 */
	4.188790, /* dimension 3 */
	4.934802, /* dimension 4 */
	5.263789, /* dimension 5 */
	5.167713, /* dimension 6 */
	4.724766, /* dimension 7 */
	4.058712, /* dimension 8 */
	3.298509, /* dimension 9 */
	2.550164, /* dimension 10 */
};
#if DIMS_NUM > 10
#error "not enough precomputed sphere volumes"
#endif

typedef double REALTYPE;

class RTree
{
public:
	/*The minimum bounding rectangle*/
	struct MBR
	{
	public:
		REALTYPE bounds[SIDES_NUM];
	public:
		bool InvalidRect()
		{
			int i;
			for(i=0;i<DIMS_NUM;i++)
			{
				if(this->bounds[i]>this->bounds[i+DIMS_NUM])
				{
					return true;
				}
			}
			return false;
		}
		/**
		* Calculate the n-dimensional volume of the bounding sphere of the rectangle
		*/
		REALTYPE GetSphericalVolume()
		{
			REALTYPE radius,sum=0,half;
			int i;
			if(this->InvalidRect())
			{
				return 0;
			}
			for(i=0;i<DIMS_NUM;i++)
			{
				half=(this->bounds[i+DIMS_NUM]-this->bounds[i])/2;
				sum+=half*half;
			}
			radius=sqrt(sum);
			return pow(radius,(REALTYPE)DIMS_NUM)*UnitSphereVolumes[DIMS_NUM];
		}
		/**
		*	Set this rectangle to invaild
		*/
		void SetInvalid()
		{
			int i;
			for(i=0;i<DIMS_NUM;i++)
			{
				this->bounds[i]=0;
				this->bounds[i+DIMS_NUM]=-1;
			}
		}
		/**
		*	Combine two rectangles and make one  to include both
		*/
		static MBR Combine(MBR *rect1,MBR *rect2)
		{
			MBR rect;
			int i,j;
			assert(rect1&&rect2);
			if(rect1->InvalidRect())
			{
				return *rect2;
			}
			if(rect2->InvalidRect())
			{
				return *rect1;
			}
			for(i=0;i<DIMS_NUM;i++)
			{
				rect.bounds[i]=MIN(rect1->bounds[i],rect2->bounds[i]);
				j=i+DIMS_NUM;
				rect.bounds[j]=MAX(rect1->bounds[j],rect2->bounds[j]);
			}
			return rect;
		}

		/**
		*	Decide whether two rectangles overlap 
		*/
		static bool Overlap(MBR *rect1,MBR *rect2)
		{
			int i,j;
			assert(rect1&&rect2);
			for(i=0;i<DIMS_NUM;i++)
			{
				j=i+DIMS_NUM;
				if(rect2->bounds[i]>rect1->bounds[j]||rect1->bounds[i]>rect2->bounds[j])
				{
					return false;
				}
			}
			return true;
		}

		/**
		*	Decide whether parent contain child
		*/
		static bool Contain(MBR *parent,MBR *child)
		{
			int i,j;
			assert(parent&&child);
			if(child->InvalidRect())
			{
				return true;
			}
			if(parent->InvalidRect())
			{
				return false;
			}
			for(i=0;i<DIMS_NUM;i++)
			{
				j=i+DIMS_NUM;
				if(child->bounds[i]<parent->bounds[i]||child->bounds[j]>parent->bounds[j])
				{
					return false;
				}
			}
			return true;
		}
		/**
		*	Decide whether two rectangles are equal with each other
		*/
		static bool Equal(MBR *rect1,MBR *rect2)
		{
			int i;
			assert(rect1&&rect2);
			for(i=0;i<SIDES_NUM;i++)
			{
				if(rect1->bounds[i]!=rect2->bounds[i])
				{
					return false;
				}
			}
			return true;
		}
		/**
		* Initialize a rectangle to have all 0 coordernates.
		*/
		void Initialize()
		{
			int i;
			for(i=0;i<DIMS_NUM;i++)
			{
				this->bounds[i]=0;
				this->bounds[i+DIMS_NUM]=0;
			}
		}
		/**
		* Initialize a rectangle to have all 0 coordernates.
		*/
		MBR()
		{
			Initialize();
		}
	};
private:
	struct Node;	//Dummy declartion
	struct Branch
	{
		MBR mbr;
		Node *child;	/*pointer to child or the obj's id*/
	public:
		/**
		*	Initialize a branch
		*/
		void Initialize()
		{
			mbr.Initialize();
			child=NULL;
		}
		/**
		*	Initialize a branch
		*/
		Branch()
		{
			this->child=NULL;
		}
	};
	struct Node
	{
	public:
		int level;	/* 0 is the leaf,>0 is the inner node */
		int count;
		Branch branch[MAX_NODES];
	public:
		/**
		*	 Return the smallest rectangle that contains all the branches in node
		*/
		MBR GetCoverRect()
		{
			int i;
			bool first=true;
			MBR rect;
			for(i=0;i<MAX_NODES;i++)
			{
				if(this->branch[i].child!=NULL)
				{
					if(first)
					{
						rect=this->branch[i].mbr;
						first=false;
					}
					else
					{
						rect=MBR::Combine(&rect,&this->branch[i].mbr);
					}
				}
			}
			return rect;
		}
		/**
		*	Destroy the node recursively
		*/
		void Destroy()
		{
			int i;
			if(this->level!=0)		//the leaf
			{
				for(i=0;i<MAX_NODES;i++)
				{
					if(branch[i].child!=NULL)
					{
						branch[i].child->Destroy();
					}
				}
			}
			for(i=0;i<MAX_NODES;i++)
			{
				branch[i].child=NULL;
				count=0;
				level=-1;
			}
			return;
		}
		/**
		*	Initialize a node
		*/
		void Initialize()
		{
			int i;
			this->level=-1;
			this->count=0;
			for(i=0;i<MAX_NODES;i++)
			{
				branch[i].Initialize();
			}
		}
		/**
		*	Initialize a node structure
		*/
		Node()
		{
			this->level=-1;
			this->count=0;
		}
	};
	class Partition
	{
	private:
		int count[2];	//The branch number in each side
		bool taken[MAX_NODES+1];		//whether a branch has been taken
		int side[MAX_NODES+1];		//which side has the branch been pick
		Branch branch[MAX_NODES+1];		//The branch collection
	private:
		/**
		*	Load the branch buffer with branches from the nodes and the extra one to p
		*/
		void _GetBranches(Node *node,Branch *br)
		{
			int i;
			int level=node->level;
			assert(node->count==MAX_NODES);
			for(i=0;i<MAX_NODES;i++)
			{
				assert(node->branch[i].child!=NULL);
				this->branch[i]=node->branch[i];
			}
			this->branch[MAX_NODES]=*br;
			node->Initialize();
			node->level=level;
		}
		/**
		*	Put the ith branch into group
		*/
		void _Classify(int i,int group)
		{
			assert(i<=MAX_NODES);
			assert(taken[i]==false);
			assert(group==0||group==1);
			taken[i]=true;
			side[i]=group;
			count[group]++;
		}
		/**
		*	Classify all the branches into two nodes
		*	Find the branch which has greatest difference vol increase
		*/
		void _ClassifyAll()
		{
			int i,j,chosen,betterGroup;
			MBR cover[2];
			_PickSeed();
			assert(count[0]=1&&count[1]==1);
			for(i=0;i<=MAX_NODES;i++)
			{
				if(taken[i]==true)
				{
					cover[side[i]]=branch[i].mbr;
				}
			}
			while(count[0]<MAX_NODES-MIN_NODES+1&&count[1]<MAX_NODES-MIN_NODES+1)
			{
				REALTYPE max=0;
				REALTYPE diff,vol[2];
				MBR temp_cover[2];
				for(i=0;i<2;i++)
				{
					vol[i]=cover[i].GetSphericalVolume();
				}
				for(i=0;i<=MAX_NODES;i++)
				{
					if(taken[i]==false)
					{
						temp_cover[0]=MBR::Combine(&cover[0],&branch[i].mbr);
						temp_cover[1]=MBR::Combine(&cover[1],&branch[i].mbr);
						diff=(temp_cover[0].GetSphericalVolume()-vol[0])-(temp_cover[1].GetSphericalVolume()-vol[1]);
						if(fabs(diff)>fabs(max))
						{
							max=diff;
							chosen=i;
							if(diff>0)
							{
								betterGroup=1;
							}
							else
							{
								betterGroup=0;
							}
						}
					}
				}
				cover[betterGroup]=MBR::Combine(&cover[betterGroup],&branch[chosen].mbr);
				_Classify(chosen,betterGroup);
			}
			if(count[0]<MAX_NODES-MIN_NODES+1)
			{
				betterGroup=0;
			}
			else
			{
				betterGroup=1;
			}
			for(i=0;i<=MAX_NODES;i++)
			{
				if(taken[i]==false)
				{
					_Classify(i,betterGroup);
				}
			}
		}
		/**
		*	Get the two branches that will waste most space to be in the same rectangle
		*/
		void _PickSeed()
		{
			int i,j,seed[2];
			REALTYPE waste,max=FLT_MIN,vol[MAX_NODES+1];
			for(i=0;i<=MAX_NODES;i++)
			{
				assert(branch[i].child!=NULL);
				vol[i]=branch[i].mbr.GetSphericalVolume();
			}
			for(i=0;i<MAX_NODES;i++)
			{
				for(j=i+1;j<=MAX_NODES;j++)
				{
					MBR cover=MBR::Combine(&branch[i].mbr,&branch[j].mbr);
					waste=cover.GetSphericalVolume()-vol[i]-vol[j];
					if(waste>max)
					{
						max=waste;
						seed[0]=i;
						seed[1]=j;
					}
				}
			}
			_Classify(seed[0],0);
			_Classify(seed[1],1);
		}
		/**
		*	Load the node n0,n1 with branches according to the  side array 
		*/
		void _LoadNodes(Node *n0,Node *n1)
		{
			int i,j;
			assert(n0&&n1);
			assert(n0->count==0&&n1->count==0);
			for(i=0;i<=MAX_NODES;i++)
			{
				assert(taken[i]==true);
				assert(side[i]==0||side[i]==1);
				if(side[i]==0)
				{
					n0->branch[n0->count++]=branch[i];
				}
				else 
				{
					n1->branch[n1->count++]=branch[i];
				}
			}
		}
	public:
		
		/**
		*	Split a node
		*	Split node to two nodes containing the new branch, and assign the new node to new_node
		*/
		void Split(Node *node,Branch *br,Node **new_node)
		{
			_GetBranches(node,br);
			_ClassifyAll();
			_LoadNodes(node,*new_node);
		}
	public:
		Partition()
		{
			int i;
			count[0]=count[1]=0;
			for(i=0;i<=MAX_NODES;i++)
			{
				taken[i]=false;
				side[i]=-1;
			}
		}
	};
private:
	Node *root;
public:
	/**
	*	Create a new rtree
	*/
	RTree()
	{
		root=new Node();
		root->level=0;
	}
	/**
	*	Destroy the rtree
	*/
	~RTree()
	{
		root->Destroy();
	}
public:
	/* The callback function when the target is hit
	*	return 0 to terminate the search process
	*/
	typedef int (*pfnSearchHitCallback)(int id,void *pfnParam);


	/**
	*	Search the whole tree  and return all the leaf rectangles that overlap the argument rectangle
	*	if pfnSHCB return 0,function will be terminated and return the current hitCount
	*/
	int Search(MBR *rect,pfnSearchHitCallback pfnSHCB,void *pfnParam)
	{
		bool terminate=false;
		return _Search(root,rect,pfnSHCB,pfnParam,&terminate);
	}

	/**
	*	Insert a rect into the rtree
	*	Return 1 if the root is split
	*	Return 0 if the root is not split
	*	level is the level to be inserted
	*/
	int Insert(MBR *rect,int id)
	{
		Node *newNode,*newRoot;
		Branch br;
		if(	_Insert(root,rect,id,&newNode,0)==1)
		{
			newRoot=new Node();
			newRoot->level=root->level+1;
			//Add the original root and the new node result from split
			br.mbr=root->GetCoverRect();
			br.child=root;
			_AddBranch(&br,newRoot,&newNode);
			br.mbr=newNode->GetCoverRect();
			br.child=newNode;
			_AddBranch(&br,newRoot,&newNode);
			root=newRoot;
			return 1;
		}
		return 0;
	}
	/**
	*	Delete a rectangle from the index tree
	*	this method provides for eliminating the tree
	*	return 0 if the target is deleted successfully
	*	return 1 if not
	*/
	int Delete(MBR *rect,int id)
	{
		queue<Node *> reinsertList;
		Node *pTempNode,*newNode;
		int i;
		assert(rect);
		if( _Delete(root,rect,id,&reinsertList)==0)
		{
			//the child with its id=id has been deleted succesfully
			//reinsert all the children in the reinsert list
			while(!reinsertList.empty())
			{
				//get the head
				pTempNode=reinsertList.front();
				reinsertList.pop();
				for(i=0;i<MAX_NODES;i++)
				{
					if(pTempNode->branch[i].child!=NULL)
					{
						_Insert(root,
							&pTempNode->branch[i].mbr,
							(int)pTempNode->branch[i].child,
							&newNode,
							pTempNode->level);
						pTempNode->branch[i].Initialize();
					}
				}
				delete pTempNode;
			}
			//if root has only one child, make the child the root
			while(root->count==1&&root->level>0)
			{
				for(i=0;i<MAX_NODES;i++)
				{
					if(root->branch[i].child)
					{
						Node *oldRoot=root;
						root=root->branch[i].child;
						delete oldRoot;
						break;
					}
				}
			}
			return 0;
		}
		return 1;
	}

private:
	/**
	*	Split a node
	*	Split node to two nodes containing the new branch, and assign the new node to new_node
	*/
	void _SplitNode(Node *node,Branch *br,Node **new_node)
	{
		Partition p;
		assert(node->count==MAX_NODES);
		*new_node=new Node();
		(*new_node)->level=node->level;
		p.Split(node,br,new_node);
	}

	/**
	* Get the branch which will have to increase least to accomodate the rect
	*/
	int _PickBranch(Node *node,MBR *rect)
	{
		int i,best=0;
		REALTYPE min=FLT_MAX,inc;
		assert(node);
		if(node->count>0)
		{
			for(i=0;i<MAX_NODES;i++)
			{
				if(node->branch[i].child!=NULL)
				{
					MBR cover=MBR::Combine(rect,&node->branch[i].mbr);
					inc=cover.GetSphericalVolume()-node->branch[i].mbr.GetSphericalVolume();
					if(min>inc)
					{
						min=inc;
						best=i;
					}
				}
			}
		}
		return best;
	}
	/**
	*	Add a branch to node;
	*	Return 0 if the node is not split
	*	Retrun 1 if the node is split and set the new node to new_node
	*/
	int _AddBranch(Branch *br,Node *node,Node **new_node)
	{
		int i;
		assert(br&&node);
		if(node->count<MAX_NODES)
		{
			for(i=0;i<MAX_NODES;i++)
			{
				if(node->branch[i].child==NULL)
				{
					node->branch[i]=*br;
					node->count++;
					return 0;
				}
			}
		}
		else
		{
			_SplitNode(node,br,new_node);
			return 1;
		}
	}
	/**
	*	Search the whole tree  and return all the leaf rectangles that overlap the argument rectangle
	*	Function will be terminated if *terminate is true
	*/
	int _Search(Node *node,MBR *rect,pfnSearchHitCallback pfnSHCB,void *pfnParam,bool *terminate)
	{
		int i,hitCount=0;
		assert(node&&rect);
		if(node->level==0)
		{
			//Already down to leaf
			for(i=0;i<MAX_NODES;i++)
			{
				if(*terminate)
				{
					return hitCount;
				}
				if(MBR::Overlap(rect,&node->branch[i].mbr))
				{
					//Hit!
					hitCount++;
					if(pfnSHCB&&pfnSHCB((int)node->branch[i].child,pfnParam)==0)
					{
						//pfnSHCH return 0 to terminate the search
						*terminate=true;
					}
				}
			}
		}
		else
		{
			//Still in the inner node
			for(i=0;i<MAX_NODES;i++)
			{
				if(*terminate)
				{
					return hitCount;
				}
				if(MBR::Overlap(rect,&node->branch[i].mbr))
				{
					hitCount+=_Search(node->branch[i].child,rect,pfnSHCB,pfnParam,terminate);
				}
			}
		}
		return hitCount;
	}
	/**
	*	Insert a rect into the index structure
	*	Return 1 if the root is split
	*	Return 0 if the root is not split
	*	level is the level to be inserted
	*/
	int _Insert(Node *node,MBR *rect,int id,Node **new_node,int level)
	{
		int i;
		Branch br;
		assert(node&&rect);
		assert(node->level>=level);
		if(node->level>level)
		{
			i=_PickBranch(node,rect);
			if(_Insert(node->branch[i].child,rect,id,new_node,level)==0)
			{
				node->branch[i].mbr=MBR::Combine(&node->branch[i].mbr,rect);
				return 0;
			}
			else
			{
				//Condense the ith child's MBR
				node->branch[i].mbr=node->branch[i].child->GetCoverRect();
				//Add the new node as a branch
				br.child=*new_node;
				br.mbr=(*new_node)->GetCoverRect();
				return _AddBranch(&br,node,new_node);
			}
		}
		else
		{
			br.child=(Node *)id;
			br.mbr=*rect;
			return _AddBranch(&br,node,new_node);
		}
		assert(false);
		return 0;	
	}
	/**
	*	Delete a rectangle with the id from the node
	*	this method provides for eliminating the tree
	*	reInsert is the queue which contains all the node need to be reinserted.
	*	return 0 if the record is found, 1 if not
	*	
	*/
	int _Delete(Node *node,MBR *rect,int id,queue<Node *> *reinsert)
	{
		int i;
		assert(node&&rect);
		if(node->level>0)
		{
			//still the inner node
			for(i=0;i<MAX_NODES;i++)
			{
				if(node->branch[i].child&&MBR::Overlap(&node->branch[i].mbr,rect))
				{
					if(_Delete(node->branch[i].child,rect,id,reinsert)==0)
					{
						//an entry has been deleted
						if(node->branch[i].child->count>=MIN_NODES)
						{
							//condense the mbr
							node->branch[i].mbr=node->branch[i].child->GetCoverRect();
						}
						else
						{
							//node has got less than MIN_NODES, so it has to be reinserted
							reinsert->push(node->branch[i].child);
							_DeleteBranch(node,i);
						}
						return 0;
					}
				}
			}
		}
		else
		{
			//Reach the leaf
			for(i=0;i<MAX_NODES;i++)
			{
				if((int)(node->branch[i].child)==id)	
				{
					//The target is found!
					_DeleteBranch(node,i);
					return 0;
				}
			}
		}
		return 1;
	}
	/**
	*	Disconnect the ith branch from the node 
	*/
	void _DeleteBranch(Node *node,int i)
	{
		assert(node);
		//Disconnect directly
		node->branch[i].Initialize();
		node->count--;
	}
};
#endif